#include "Periodic.h"
#include "arduino/WProgram.h"
#include "MsTimer2/MsTimer2.h"
#include "Protocol.h"
#include "Reset.h"

bool WatchdogFed;
int WatchdogWarning;

bool EStopped;

bool RequestToStream;
InputStreamMode AnalogPinStreamMode[6];
InputStreamMode DigitalPinStreamMode[14];
uint8_t AnalogPreviousValue[6];
bool DigitalPreviousValue[14];

void Handler()
{
	interrupts();

	if (EStopped)
	{
		SendNotification(NotificationEStop);
	}
	else
	{
		// if not fed for two loops, will reset arduino
		// feeding sets fed to 1 and warning to 0
		if(WatchdogFed == false)
		{
			if(WatchdogWarning >= 9) // On 10th time, resets
				ResetArduino();
			else
				WatchdogWarning++;
		}
		else
			WatchdogFed = false;

		RequestToStream = true;
	}

	MsTimer2::stop();
	MsTimer2::start();
}

void SetEStopped(bool eStopped)
{
	EStopped = eStopped;
}

void StartPeriodic()
{
	SetEStopped(false);

	for(int i = 0; i < 6; i++)
		AnalogPinStreamMode[i] = Never;
	for(int i = 0; i < 14; i++)
		DigitalPinStreamMode[i] = Never;
	WatchdogReset();
	MsTimer2::set(100, Handler);
	MsTimer2::start();
}

void StopPeriodic()
{
	MsTimer2::stop();
	WatchdogWarning = 0;
	for(int i = 0; i < 14; i++)
			DigitalPinStreamMode[i] = Never;
	for(int i = 0; i < 6; i++)
			AnalogPinStreamMode[i] = Never;
}

void WatchdogReset()
{
	WatchdogFed = true;
	WatchdogWarning = 0;
}

bool GetRequestToStream()
{
	return RequestToStream;
}

void Stream()
{
	RequestToStream = false;
	for(uint8_t i = 0; i < 6; i++)
	{
		uint8_t pinvalue = analogRead(i);
		switch(AnalogPinStreamMode[i])
		{
		case Never:
			break;
		case OnChange:
			if(pinvalue != AnalogPreviousValue[i])
				SendAnalogInput(i, pinvalue);
			break;
		case Always:
			SendAnalogInput(i, pinvalue);
			break;
		}
		AnalogPreviousValue[i] = pinvalue;
	}
	for(uint8_t i = 0; i < 14; i++)
	{
		bool pinvalue = digitalRead(i);
		switch(DigitalPinStreamMode[i])
		{
		case Never:
			break;
		case OnChange:
			if(pinvalue != DigitalPreviousValue[i])
				SendDigitalInput(i, pinvalue);
			break;
		case Always:
			SendDigitalInput(i, pinvalue);
			break;
		}
		DigitalPreviousValue[i] = pinvalue;
	}
}

void EnableAnalogStream(uint8_t pin, InputStreamMode mode)
{
	if(pin < 6)
		AnalogPinStreamMode[pin] = mode;
}

void EnableDigitalStream(uint8_t pin, InputStreamMode mode)
{
	if(pin < 14)
		DigitalPinStreamMode[pin] = mode;
}
